import os
import sys

import numpy as np
import pandas as pd

np.set_printoptions(suppress=True)

import numpy as np
from scipy.optimize import leastsq


def four_param_conversion(filepath, points_array):
    points_list = list(map(int, points_array.split(',')))
    org_x1, org_y1, org_z1, org_x2, org_y2, org_z2, tgt_x1, tgt_y1, tgt_z1, tgt_x2, tgt_y2, tgt_z2 = points_list
    # 输入原始点的XYZ属性
    original_points = np.array([[org_x1, org_y1], [org_x2, org_y2]])
    # 输入目标点的XYZ属性
    target_points = np.array([[tgt_x1, tgt_y1], [tgt_x2, tgt_y2]])

    # 定义四参数模型函数
    def four_param_model(params, x):
        Tx, Ty, theta = params
        x_rot = x[:, 0] * np.cos(theta) - x[:, 1] * np.sin(theta)
        y_rot = x[:, 0] * np.sin(theta) + x[:, 1] * np.cos(theta)
        x_trans = x_rot + Tx
        y_trans = y_rot + Ty
        return np.column_stack((x_trans, y_trans))

    # 定义误差函数
    def error_func(params, x, y):
        transformed_point = four_param_model(params, x)
        residual = transformed_point - y
        return residual.flatten()

    # 初始参数猜测值
    init_params = np.zeros(3)
    init_params[:2] = 0.1 * np.mean(target_points - original_points, axis=0)

    # 通过最小二乘法拟合参数
    opt_params, _ = leastsq(error_func, init_params, args=(original_points, target_points))

    # 输出四参数
    Tx, Ty, theta = opt_params
    print("平移: Tx={}, Ty={}".format(Tx, Ty))
    print("旋转: theta={}".format(theta))
    print("尺度因子：1")

    # 读入XY数据，进行参数转换
    df = pd.read_excel('临时坐标.xlsx')
    dataframe = pd.DataFrame()
    dataframe['X'] = df['X']
    dataframe['Y'] = df['Y']
    dataframe['Z'] = df['Z']
    points = dataframe.values

    # 将原始点进行四参数转换
    convert_points = four_param_model(opt_params, points)
    transformed_point = four_param_model(opt_params, target_points)

    tempdf = pd.DataFrame(convert_points, columns=['X', 'Y'])

    data = pd.DataFrame()
    data['点号'] = df['点号']
    data['转换前X'] = df['X']
    data['转换前Y'] = df['Y']
    data['转换后X'] = tempdf['X']
    data['转换后Y'] = tempdf['Y']
    data['Z'] = df['Z']

    # 计算残差
    residuals = transformed_point - target_points

    # 计算坐标转换精度（残差的均方根误差）
    rmse = np.sqrt(np.mean(residuals ** 2))
    data = data.assign(RMSE="")
    data.iloc[0, 6] = rmse
    write_data(data, filepath + "\四参数转换", "Xlsx格式")
    # print("坐标更正后的点:", transformed_points)
    print("残差的均方根误差RMSE:", rmse)


def seven_param_conversion(filepath, points_array):
    points_list = list(map(int, points_array.split(',')))
    org_x1, org_y1, org_z1, \
        org_x2, org_y2, org_z2, \
        org_x3, org_y3, org_z3, \
        tgt_x1, tgt_y1, tgt_z1, \
        tgt_x2, tgt_y2, tgt_z2, \
        tgt_x3, tgt_y3, tgt_z3 = points_list
    # 输入原始点的XYZ属性
    original_points = np.array([[org_x1, org_y1, org_z1],
                                [org_x2, org_y2, org_z2],
                                [org_x3, org_y3, org_z3]])

    # 输入目标点的XYZ属性
    target_points = np.array([[tgt_x1, tgt_y1, tgt_z1],
                              [tgt_x2, tgt_y2, tgt_z2],
                              [tgt_x3, tgt_y3, tgt_z3]])

    # 定义七参数模型函数
    def seven_param_model(params, x):
        Tx, Ty, Tz, Rx, Ry, Rz = params
        x_rot = x[:, 0] * (1 + Rz) - x[:, 1] * Ry + x[:, 2] * Rx
        y_rot = x[:, 0] * Rz + x[:, 1] * (1 + Rx) - x[:, 2] * Ry
        z_rot = -x[:, 0] * Ry + x[:, 1] * Rx + x[:, 2] * (1 + Rz)
        x_trans = x_rot + Tx
        y_trans = y_rot + Ty
        z_trans = z_rot + Tz
        return np.column_stack((x_trans, y_trans, z_trans))

    # 定义误差函数
    def error_func(params, x, y):
        transformed_points = seven_param_model(params, x)
        residuals = transformed_points - y
        return residuals.flatten()

    # 初始参数猜测值
    init_params = np.zeros(6)

    # 使用原始点和目标点坐标的平均差异的一小部分作为平移参数的初始猜测值
    init_params[:3] = 0.1 * np.mean(target_points - original_points, axis=0)

    # 通过最小二乘法拟合参数
    opt_params, _ = leastsq(error_func, init_params, args=(original_points, target_points))

    # 输出七参数
    Tx, Ty, Tz, Rx, Ry, Rz = init_params[0], init_params[1], init_params[2], opt_params[3], opt_params[4], opt_params[5]
    print("平移: Tx={}, Ty={}, Tz={}".format(Tx, Ty, Tz))
    print("旋转: Rx={}, Ry={}, Rz={}".format(Rx, Ry, Rz))
    print("尺度因子：1")

    # 读入XY数据，进行参数转换
    df = pd.read_excel('临时坐标.xlsx')
    dataframe = pd.DataFrame()
    dataframe['X'] = df['X']
    dataframe['Y'] = df['Y']
    dataframe['Z'] = df['Z']
    points = dataframe.values

    # 将原始点进行七参数转换
    transformed_points = seven_param_model(init_params, original_points)
    convert_points = seven_param_model(init_params, points)

    tempdf = pd.DataFrame(convert_points, columns=['X', 'Y', 'Z'])

    data = pd.DataFrame()
    data['点号'] = df['点号']
    data['转换前X'] = df['X']
    data['转换前Y'] = df['Y']
    data['转换前Z'] = df['Z']
    data['转换后X'] = tempdf['X']
    data['转换后Y'] = tempdf['Y']
    data['转换后Z'] = tempdf['Z']

    # 计算残差
    residuals = transformed_points - target_points

    # 计算坐标转换精度（残差的均方根误差）
    rmse = np.sqrt(np.mean(residuals ** 2))
    data = data.assign(RMSE="")
    data.iloc[0, 7] = rmse
    write_data(data, filepath + "\七参数转换", "Xlsx格式")

    # print("坐标更正后的点:", transformed_points)
    print("残差的均方根误差RMSE:", rmse)


def write_data(data, file_path, type):
    if type == "Xlsx格式":
        # 将数据写入excel文件，如果文件不存在则创建一个新文件，已存在则覆盖
        # 指定excel文件路径
        excel_path = file_path + '.xlsx'
        # 判断文件是否存在
        if os.path.isfile(excel_path):
            # 文件存在，删除原文件
            os.remove(excel_path)
        # 将数据写入CSV文件
        data.to_excel(excel_path, index=False)
    elif type == "Csv格式":
        # 将数据写入CSV文件，如果文件不存在则创建一个新文件，已存在则覆盖
        # 指定CSV文件路径
        csv_path = file_path + '.csv'
        # 判断文件是否存在
        if os.path.isfile(csv_path):
            # 文件存在，删除原文件
            os.remove(csv_path)
        # 将数据写入CSV文件
        data.to_csv(csv_path, index=False)
    else:
        print("请选择正确的输出格式")


def cal_four_seven_param(choose, output_file_folder):
    # 根据选择，判断进行四参数求解还是七参数求解
    if choose == "四参数":
        four_param_conversion(output_file_folder, sys.argv[19])
    elif choose == "七参数":
        seven_param_conversion(output_file_folder, sys.argv[19])


# if __name__ == '__main__':
    # four_param_conversion("result",
    #                       4312820.897, 513670.931,
    #                       4314058.425, 514231.984,
    #                       4312820.597, 513670.831,
    #                       4314058.225, 514231.784)
    #
    # seven_param_conversion("result",
    #                        3381400.980, 395422.030, 13.956,
    #                        3381404.344, 395844.239, 13.207,
    #                        3382149.810, 396003.592, 13.290,
    #                        3381400.680, 395422.430, 13.856,
    #                        3381404.244, 395844.539, 13.107,
    #                        3382149.610, 396003.892, 13.190
    #                        )
